//
// Created by xujingyi on 2021/4/28.
//

#ifndef ALLIANCE_SENTRY_V2_0_ARMOR_FINDER_H
#define ALLIANCE_SENTRY_V2_0_ARMOR_FINDER_H

#include <solve/Predictor.h>
#include "Serial.h"
#include "find/armor_class.h"
#include "find/CNN.h"
#include "solve/solve_PNP.h"
#include "solve/trajectory_resolve.h"

extern rm::Setter rm_setter;
namespace rm {
/*!
 * 与颜色有关的识别参数
 */
class ColorArmorParam{
public:
    int brightness_threshold_val;
    int color_channel_threshold_val;
    int color_diff_threshold_val;
    int marker_min_peri=rm_setter.marker_min_peri;
    int marker_max_peri=rm_setter.marker_max_peri;
    int marker_min_height_width_ratio=rm_setter.marker_min_height_width_ratio;
    int marker_max_height_width_ratio=rm_setter.marker_max_height_width_ratio;
    ColorArmorParam()=default;
    ~ColorArmorParam()=default;
};

class BlueArmorParam :public ColorArmorParam{
public:
    ////Pre-treatment:默认为蓝色
    BlueArmorParam() {
        ColorArmorParam::brightness_threshold_val = rm_setter.blue_brightness_threshold_val;
        ColorArmorParam::color_diff_threshold_val = rm_setter.blue_color_diff_threshold_val;
        ColorArmorParam::color_channel_threshold_val=rm_setter.blue_color_channel_threshold_val;
    };
    ~BlueArmorParam()=default;
};

class RedArmorParam :public ColorArmorParam {
public:
    RedArmorParam() {
        ColorArmorParam::brightness_threshold_val = rm_setter.red_brightness_threshold_val;
        ColorArmorParam::color_diff_threshold_val = rm_setter.red_color_diff_threshold_val;
        ColorArmorParam::color_channel_threshold_val=rm_setter.red_color_channel_threshold_val;
    };
    ~RedArmorParam()=default;
};

/*!
 * 装甲识别工具箱
 */
class ArmorFinder {
public:
    typedef enum {
        SEARCHING_STATE, TRACKING_STATE, GYRO_STATE     //自瞄状态枚举定义
    } State;

    ArmorFinder()=default;
    ArmorFinder(ArmorColor color, Serial &u) : _enemy_color(color), state(SEARCHING_STATE), serial(u) {};
    ~ArmorFinder() = default;

    Serial &serial;                                     // 串口对象，引用外部变量，用于和能量机关共享同一个变量
    State last_state,state;
    rm::ArmorState track_state=rm::CHANGE_ARMOR;
    rm::ChassisState chassis_state=rm::UNIFORM;
    //last_track_armor:when lose_then_keep_cnt not upon limit(you just haven't find the same target for seconds),
    // it still will reserve the target you tracked next time;else it will be cleared
    rm::Armor last_track_armor=Armor(),target_armor=Armor(),last_armor=Armor();
    rm::CNN lenet;
    rm::SolvePNP pnp;
    rm::TrajectoryResolve trajectory;
    Eigen::Matrix<float,3,3> R_vec,R_back;
    GyroTool gyro_tool;
    int box_anti_switch_cnt=0,keep_in_one_box_cnt=0;

    void findTarget(cv::Mat &src);//便于进行避开串口接收的线程
    void solvePosNSend(cv::Mat &src);//2中pixel_predict有使用串口数据
    void setEnemyColor(ArmorColor color){_enemy_color=color;}//设置敌方颜色
    ArmorColor getEnemyColor() const {return _enemy_color;}//获取敌方颜色
    bool setFindLastTargetFlag(const bool &flag){_find_last_target_flag=flag; return _find_last_target_flag;}
    bool getFindLastTargetFlag() const {return _find_last_target_flag;}
    cv::Rect2f getTrackBox() const {return _track_box;};
    void setShootflag(bool flag){ _shootflag=flag;};
    bool getShootflag(){return _shootflag;};
//        void transGim2Robo(cv::Point3f &posion, float pitch, float yaw);//pitch,yaw hudu,
//        cv::Point2f transRobo2Gim(cv::Point3f &posion);

private:
    cv::Rect2f _track_box=cv::Rect2f(0,0,PICTURE_COL_NUM,PICTURE_ROW_NUM);
    rm::Armors _armors;
    ArmorColor _enemy_color = BLUE;
    ColorArmorParam *param;
    bool _find_last_target_flag=false;//用于标志是否搜索到目标，用于统一track和gyro模式对同一变量的追踪
    bool _spinning_state_flag=false;
    bool _shootflag=false;
//        uint8_t highlandNUM_shootflag=false;
//        uint8_t enemyothersshootable=0x00;

    cv::Mat preProcess(const cv::Mat &src) ;//预处理(二值化)：
    std::vector <Marker> findMarker(const cv::Mat &track_src,const cv::Mat &img2show);//找灯条
    MarkerCouples matchMarkerCouple(const std::vector <Marker> &markers,const cv::Mat& img2show);//灯条匹配(就近匹配，滑动窗口)
    Armors findArmor(rm::MarkerCouples &couples,const cv::Mat &track2show,const cv::Mat &track_origin,const cv::Point2f offset);
    cv::Rect2f _setNextTrackBox(const rm::Armor &armor);//Tracking模式下的设置tracking box
    cv::Rect2f _setNextGyroBox(const rm::Armors &armors);//Tracking模式下的设置tracking box
    int judgeNum(cv:: Mat &num_pic,const bool is_highland_blind_view);//数字识别（不同pb文件）(截好图再投)
    Armor decision(rm::Armors &armors,const cv::Mat src) ;//目标决策
    Armor gyrodecision(rm::Armors &armors,const cv::Mat src);
    cv::Point3f posResolve(Armor& armor);//位姿解算
    cv::Point3f pre4Normal();
    cv::Point3f pre4Gyro(rm::Armors& gyro_armors,cv::Point2f offset);
    cv::Point3f pixelAngleSolve(const cv::Point2f pic_center,rm::Armor &armor,const float distance);

    void _calcRotMatrix();
//
bool judgeSpinning();//判断是否为小陀螺模式:pos has changeed a lot for several times
rm::Armors searchingTarget(const cv::Mat &src,const cv::Point2f offset);  //searching state主函数
bool stateSearchingTarget(cv::Mat &src);  //searching state主函数
bool stateTrackingTarget(cv::Mat &src);  //tracking state主函数
bool stateSpinningTarget(cv::Mat &src);                // spinning state主函数:wait for first mark come out and shoot,then go to middle,then back to first
};
}
#endif //ALLIANCE_SENTRY_V2_0_ARMOR_FINDER_H
